///
/// AEye, Inc. ("COMPANY") CONFIDENTIAL
/// Unpublished Copyright (c) 2022 AEye, Inc., All Rights Reserved.
///
/// NOTICE: All information contained herein is, and remains the property of COMPANY. The intellectual and technical concepts contained
/// herein are proprietary to COMPANY and may be covered by U.S. and Foreign Patents, patents in process, and are protected by trade secret or copyright law.
/// Dissemination of this information or reproduction of this material is strictly forbidden unless prior written permission is obtained
/// from COMPANY. Access to the source code contained herein is hereby forbidden to anyone except current COMPANY employees, managers or contractors who have executed
/// Confidentiality and Non-disclosure agreements explicitly covering such access.
///
/// The copyright notice above does not evidence any actual or intended publication or disclosure of this source code, which includes
/// information that is confidential and/or proprietary, and is a trade secret, of COMPANY. ANY REPRODUCTION, MODIFICATION, DISTRIBUTION, PUBLIC PERFORMANCE,
/// OR PUBLIC DISPLAY OF OR THROUGH USE OF THIS SOURCE CODE WITHOUT THE EXPRESS WRITTEN CONSENT OF COMPANY IS STRICTLY PROHIBITED, AND IN VIOLATION OF APPLICABLE
/// LAWS AND INTERNATIONAL TREATIES. THE RECEIPT OR POSSESSION OF THIS SOURCE CODE AND/OR RELATED INFORMATION DOES NOT CONVEY OR IMPLY ANY RIGHTS
/// TO REPRODUCE, DISCLOSE OR DISTRIBUTE ITS CONTENTS, OR TO MANUFACTURE, USE, OR SELL ANYTHING THAT IT MAY DESCRIBE, IN WHOLE OR IN PART.
///

#include <iostream>
#include <fstream>
#include <cstdio>
#include <ctime>
#include <cmath>

#include "SensorListener.hpp"
#include "PointCloudFrame.hpp"
#include <cassert>

using namespace aeye;

void SensorListener::addSensor(const std::string& netInterface, const std::string& ipAddress) {
  auto sensor = new Listener(netInterface, ipAddress, ipAddress);
  sensor->setOnPointCloudFrameCallback(std::bind(&SensorListener::processRangeDataCallback, this, std::placeholders::_1));
  sensor->setOnDiagnosticDataCallback(std::bind(&SensorListener::processDiagnosticDataCallback, this, std::placeholders::_1));
  _sensorMap[ipAddress]= sensor;
}

void SensorListener::addSensor(const std::string& netInterface, const std::string& ipAddress, uint16_t commandPort,
        uint16_t rangePort, uint16_t imagePort, uint16_t serverRangePort, uint16_t serverImagePort) {
  auto sensor = new Listener(netInterface, ipAddress, ipAddress, commandPort, rangePort, imagePort, serverRangePort,
                           serverImagePort);
  sensor->setOnPointCloudFrameCallback(std::bind(&SensorListener::processRangeDataCallback, this, std::placeholders::_1));
  sensor->setOnDiagnosticDataCallback(std::bind(&SensorListener::processDiagnosticDataCallback, this, std::placeholders::_1));
  _sensorMap[ipAddress]= sensor;
}

void SensorListener::disconnectFromSensors() {
  for (const auto& sensor : _sensorMap) {
    try{
      sensor.second->disconnect(); // disconnect from the actual sensor.
    }
    catch (std::exception& e){
      std::cerr << "Disconnect to Sensor " << sensor.second->getSensorIp()
                << " failed with error " << e.what() << "!" << std::endl;
    }
  }
};

void SensorListener::startSensors() {
  for (const auto& sensor : _sensorMap){
      sensor.second->start();
      sensor.second->startDiagnostic();
  }
}

void SensorListener::stopSensors() {
  for (const auto& sensor : _sensorMap){
    sensor.second->stop();
  }
}

/* LiDAR data stream callback
 *
 * User code that works on the point cloud data can be called here when a new frame arrives.
 * The example code here is designed to call a function that writes out a csv-format file.
 */
void SensorListener::processRangeDataCallback(PointCloudFrame&& frame) {
  std::cout << "RangingFrame with Id=" << frame.getFrameId() << " received from " << frame.getIpAddress() << "!" << std::endl;

  if (frame.getFrameId()==0) {
    return;
  }

  // Check if this is the first frame received
  if (!firstPointCloudFrameEncountered) {
	  firstPointCloudFrameEncountered = true;
	  frameIdOfFirstPointCloudFrame = frame.getFrameId();
  }

  // After the stream has started, save out the first few point cloud frames as csv files with the dumpCsv() routine
  std::cerr << "frameIdOfFirstPointCloudFrame = " << frameIdOfFirstPointCloudFrame << "\tframe.getFrameId() = " << frame.getFrameId() << "\n\n";
  if((frame.getFrameId() >= frameIdOfFirstPointCloudFrame) && (frame.getFrameId() < frameIdOfFirstPointCloudFrame + numFramesToCapture)){
    dumpCsv(frame);
  }
}

void SensorListener::processDiagnosticDataCallback(DiagnosticData&& diagData) {
  std::cout << "-------------------EXAMPLE DIAGNOSTIC DATA---------------------" << std::endl;
  std::cout << "Telemetry Revision: " << diagData.telemRev << std::endl;
  std::cout << "Board Temperature: "<< reinterpret_cast<float&>(diagData.brdTempCels) << "C" << std::endl;
  std::cout << "Laser Temperature: "<< reinterpret_cast<float&>(diagData.lsrTempCels) << "C" << std::endl;
  std::cout << "Scanhead Temperature: "<< reinterpret_cast<float&>(diagData.scanheadTempCels) << "C" << std::endl;
  std::cout << "Scanhead Relative Humidity: "<< reinterpret_cast<float&>(diagData.scanheadRh) << "%" << std::endl;
  std::cout << "Scanhead Window Temperature: "<< reinterpret_cast<float&>(diagData.scanWindowTempCels) << "C" << std::endl;
  std::cout << "Scanhead Window Relative Humidity: "<< reinterpret_cast<float&>(diagData.scanWindowRh) << "%" << std::endl;
  std::cout << "Board Temperature near ADC: "<< reinterpret_cast<float&>(diagData.brdTempNearAdcCels) << "C" << std::endl;
  std::cout << "ADC Temperature: "<< reinterpret_cast<float&>(diagData.adcTempCels) << "C" << std::endl;

  std::cout << "*******PRINTING EXAMPLE FOR IRPS DEVICE 0 GLOBAL DATA********" << std::endl;
  std::cout << "Irps Dev0 Global Data, Global Vcc: "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsGlobPkt.globVcc) << "V"<< std::endl;
  std::cout << "Irps Dev0 Global Data, Global Vdrv : "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsGlobPkt.globVdrv) << "V" << std::endl;
  std::cout << "Irps Dev0 Global Data, Total Input Power: "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsGlobPkt.globTotalInputPwr) << "W" << std::endl;
  std::cout << "Irps Dev0 Global Data, Global Packet Count: "  << diagData.irpsDev0Packet.irpsGlobPkt.pktCt << std::endl;

  std::cout << "*******PRINTING EXAMPLE FOR IRPS DEVICE 0 CHANNEL 0********" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chVin: " << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chVin) << "V" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chIin: " << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chIin) << "A" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chPin: "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chPin) << "W" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chVout: "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chVout) << "V" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chIout: "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chIout) << "A" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chPout: " << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chPout) << "W" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chTemp: "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chTemp) << "C" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chVoutPeak: "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chVoutPeak)<< "V" << std::endl;
  std::cout << "Irps Dev0 Channel 0, chIoutPeak: " << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chIoutPeak) << "A"<< std::endl;
  std::cout << "Irps Dev0 Channel 0, chTempPeak: "  << reinterpret_cast<float&>(diagData.irpsDev0Packet.irpsPerChPkt0.chTempPeak) << "C" << std::endl;
  std::cout << "Irps Dev0 Channel 0, pktCt: "  << diagData.irpsDev0Packet.irpsPerChPkt0.pktCt << std::endl;

}

/* dumpCsv routine
 *
 * This routine will write out a single csv-format file when called.  It takes as an input
 * a reference to the point cloud frame.  It will then extract the frame ID and add it
 * to the output file name it constructs. It will include in the csv output file all
 * active point types (selected earlier when basicControlsDemo() function was called).
 */
void SensorListener::dumpCsv(const PointCloudFrame& frame) {

  std::string nframe = "pntcloud_" + std::to_string(frame.getFrameId()) + ".csv"; // create output file name

  std::ofstream csvFile;
  csvFile.open(nframe);

  uint16_t returnFlags = frame.getPointReturnFlags();   // get the point types that are active

  const bool printCartesian         = (returnFlags & aeye::PointReturnFlags::UNUSED_0) != 0;
  const bool printSpherical         = (returnFlags & aeye::PointReturnFlags::SPHERICAL_COORDINATES) != 0;
  const bool printTsOffset          = (returnFlags & aeye::PointReturnFlags::UNUSED_2) != 0;
  const bool printIntensityTypeData = (returnFlags & aeye::PointReturnFlags::INTENSITY) != 0;
  const bool printIntensityExt      = (returnFlags & aeye::PointReturnFlags::UNUSED_8) != 0;
  const bool printColorRGB          = (returnFlags & aeye::PointReturnFlags::UNUSED_6) != 0;
  const bool printType              = (returnFlags & aeye::PointReturnFlags::UNUSED_7) != 0;
  const bool printLaserPixel        = (returnFlags & aeye::PointReturnFlags::UNUSED_1) != 0;
  const bool printReceiverPixel     = (returnFlags & aeye::PointReturnFlags::UNUSED_5) != 0;
  const bool printChargeTime        = (returnFlags & aeye::PointReturnFlags::UNUSED_3) != 0;

  csvFile << "Sensor IP, Frame ID";
  if (printCartesian)          csvFile << ", x (m), y (m), z (m)";
  if (printSpherical)          csvFile << ", range (m), azimuth (degrees), elevation (degrees)";
  if (printTsOffset)           csvFile << ", point time stamp (ns), frame time stamp (s), frame time stamp (us)";
  if (printIntensityTypeData)  csvFile << ", raw intensity, quasi reflectivity";
  if (printIntensityExt)       csvFile << ", peak width, hg-lg ratio";
  if (printColorRGB)           csvFile << ", r, g, b";
  if (printLaserPixel)         csvFile << ", laser row, laser col, scan direction";
  if (printReceiverPixel)      csvFile << ", pixel row, pixel col";
  if (printChargeTime)         csvFile << ", actual charge (ns)";
  if (printType)               csvFile << ", point type, multi-echo return";
  csvFile << '\n';

  const auto& ip = frame.getIpAddress();
  const auto frame_id = frame.getFrameId();

  for (const auto& point : frame.getPoints()) {
    csvFile << ip << ", " << frame_id;
    if (printCartesian)          csvFile << ", " << point.pointReturnCartesianCoords.x
                                         << ", " << point.pointReturnCartesianCoords.y
                                         << ", " << point.pointReturnCartesianCoords.z;
    if (printSpherical)          csvFile << ", " << point.pointReturnSphericalCoords.radius_scaled
                                         << ", " << static_cast<float>(point.pointReturnSphericalCoords.azimuth) / 10000.f * 180.0 / M_PI
                                         << ", " << static_cast<float>(point.pointReturnSphericalCoords.elevation) / 10000.f * 180.0 / M_PI;
    if (printTsOffset)           csvFile << ", " << point.pointReturnTimeStampOffset.time_offset
                                         << ", " << frame.getTimeSec()
                                         << ", " << frame.getTimeuSec();
    if (printIntensityTypeData)  csvFile << ", " << point.pointReturnIntensity.intensity_type_data;
    if (printIntensityExt)       csvFile << ", " << point.pointReturnIntensityExt.peak_width
                                         << ", " << point.pointReturnIntensityExt.hg_lg_ratio;
    if (printColorRGB)           csvFile << ", " << (int)point.pointReturnRGB.r
                                         << ", " << (int)point.pointReturnRGB.g
                                         << ", " << (int)point.pointReturnRGB.b;
    if (printLaserPixel)         csvFile << ", " << point.pointReturnLixel.row
                                         << ", " << point.pointReturnLixel.col
                                         << ", " << (int)point.pointReturnLixel.flags;
    if (printReceiverPixel)      csvFile << ", " << point.pointReturnPixel.y
                                         << ", " << point.pointReturnPixel.x;
    if (printChargeTime)         csvFile << ", " << (int)point.pointReturnChargeTime.actual_charge_time;
    if (printType) {
      csvFile << ", " << std::hex << point.pointReturnType.point_return_mask << std::dec;
      switch (static_cast<uint8_t>(point.pointReturnType.point_return_mask & 0x3U)) {
        case 0U: csvFile << ", " << "FIRST_ECHO"; break;
        case 1U: csvFile << ", " << "SECOND_ECHO"; break;
        case 2U: csvFile << ", " << "THIRD_ECHO"; break;
        case 3U: csvFile << ", " << "FOURTH_ECHO"; break;
        default: csvFile << ", " << "UNKNOWN_ECHO";
      }
    }
    csvFile << '\n';
  }

}